#include "serialport.h"
#include <sstream>
#include <vector>
#include <ros/package.h>
#include <iomanip>
#include <cstring>
#include <errno.h>
#include <unistd.h>

using json = nlohmann::json;

SerialPort::SerialPort(const std::string& port, int baud_rate)
    : port_(port), baud_rate_(baud_rate), fd_(-1) {
    
    // 初始化数据类型映射
    initializeDataTypes();
}

void SerialPort::initializeDataTypes() {
    // 尝试从配置文件加载数据类型映射
    std::string configFile = ros::package::getPath("pegasus") + "/config/mcu_upstream_list.yaml";
    
    try {
        loadUpstreamConfig(configFile);
        ROS_INFO("已从配置文件加载 %lu 个数据键类型映射", keyDataTypes_.size());
    } catch (const std::exception& e) {
        ROS_WARN("加载配置文件失败: %s，使用默认配置", e.what());
        
        // 如果配置文件加载失败，使用默认的硬编码配置
        keyDataTypes_["button_basic"] = "double";
        keyDataTypes_["button_adv"] = "double";
        keyDataTypes_["button_cam_calib"] = "double";
        keyDataTypes_["sensor_voltage"] = "double";
        keyDataTypes_["sensor_current"] = "double";
        keyDataTypes_["sensor_temperature"] = "double";
        keyDataTypes_["device_status"] = "string";
        keyDataTypes_["error_message"] = "string";
        keyDataTypes_["firmware_version"] = "string";
        
        ROS_INFO("已初始化 %lu 个默认数据键类型映射", keyDataTypes_.size());
    }
}

SerialPort::~SerialPort() {
    closePort();
}

bool SerialPort::openPort() {
    ROS_INFO("正在打开串口: %s, 波特率: %d", port_.c_str(), baud_rate_);
    
    fd_ = open(port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd_ == -1) {
        ROS_ERROR("无法打开端口 %s: %s", port_.c_str(), strerror(errno));
        return false;
    }

    struct termios options;
    if (tcgetattr(fd_, &options) != 0) {
        ROS_ERROR("获取串口属性失败: %s", strerror(errno));
        close(fd_);
        fd_ = -1;
        return false;
    }
    
    // 设置波特率
    speed_t speed;
    switch (baud_rate_) {
        case 9600: speed = B9600; break;
        case 19200: speed = B19200; break;
        case 38400: speed = B38400; break;
        case 57600: speed = B57600; break;
        case 115200: speed = B115200; break;
        case 230400: speed = B230400; break;
        default:
            ROS_ERROR("不支持的波特率: %d", baud_rate_);
            close(fd_);
            fd_ = -1;
            return false;
    }
    
    cfsetispeed(&options, speed);
    cfsetospeed(&options, speed);
    
    // 配置数据位、停止位、校验位
    options.c_cflag &= ~PARENB;   // 无校验位
    options.c_cflag &= ~CSTOPB;   // 1个停止位
    options.c_cflag &= ~CSIZE;    // 清除数据位设置
    options.c_cflag |= CS8;       // 8数据位
    options.c_cflag |= (CLOCAL | CREAD); // 忽略调制解调器控制线，使能接收
    
    // 禁用硬件流控制
    options.c_cflag &= ~CRTSCTS;
    
    // 配置输入模式
    options.c_iflag &= ~(IXON | IXOFF | IXANY); // 禁用软件流控制
    options.c_iflag &= ~(INLCR | ICRNL);        // 禁用换行符转换
    options.c_iflag &= ~ISTRIP;                 // 不剥离第8位
    
    // 配置输出模式
    options.c_oflag &= ~OPOST;    // 禁用输出处理
    options.c_oflag &= ~ONLCR;    // 禁用换行符转换
    
    // 配置本地模式
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // 原始模式
    
    // 设置读取超时
    options.c_cc[VMIN] = 0;   // 最小读取字符数
    options.c_cc[VTIME] = 1;  // 读取超时时间(0.1秒单位)
    
    if (tcsetattr(fd_, TCSANOW, &options) != 0) {
        ROS_ERROR("设置串口属性失败: %s", strerror(errno));
        close(fd_);
        fd_ = -1;
        return false;
    }
    
    // 清空串口缓冲区
    tcflush(fd_, TCIOFLUSH);
    
    ROS_INFO("串口 %s 打开成功，文件描述符: %d", port_.c_str(), fd_);
    return true;
}

void SerialPort::closePort() {
    if (fd_ != -1) {
        close(fd_);
        fd_ = -1;
    }
}

bool SerialPort::sendData(const std::string& data) {
    if (fd_ == -1) {
        ROS_ERROR("串口未打开，无法发送数据");
        return false;
    }
    
    // 添加调试信息
    ROS_INFO("发送数据长度: %lu 字节", data.size());
    ROS_INFO("发送内容: %s", data.c_str());
    
    // 打印十六进制格式
    std::stringstream hexStream;
    for (unsigned char c : data) {
        hexStream << "\\x" << std::hex << std::uppercase << std::setw(2) 
                 << std::setfill('0') << static_cast<int>(c) << " ";
    }
    ROS_INFO("十六进制: %s", hexStream.str().c_str());
    
    // 添加换行符确保MCU能够正确接收完整数据包
    std::string dataWithNewline = data + "\n";
    
    ssize_t bytes_written = write(fd_, dataWithNewline.c_str(), dataWithNewline.size());
    if (bytes_written < 0) {
        ROS_ERROR("写入串口失败: %s", strerror(errno));
        return false;
    }
    
    if (bytes_written != static_cast<ssize_t>(dataWithNewline.size())) {
        ROS_WARN("写入字节数不匹配: 期望 %lu, 实际 %ld", 
                 dataWithNewline.size(), bytes_written);
    }
    
    // 强制刷新串口缓冲区
    fsync(fd_);
    
    ROS_INFO("成功发送 %ld 字节数据", bytes_written);
    return bytes_written > 0;
}

bool SerialPort::sendBin(const char* data, int len) {
    return fd_ != -1 && write(fd_, data, len) == len;
}

std::string SerialPort::receiveData(int timeout_ms) {
    if (fd_ == -1) return "";

    fd_set read_fds;
    struct timeval timeout;
    
    FD_ZERO(&read_fds);
    FD_SET(fd_, &read_fds);
    
    timeout.tv_sec = timeout_ms / 1000;
    timeout.tv_usec = (timeout_ms % 1000) * 1000;

    std::string result;
    char buffer[256];
    int rv = select(fd_ + 1, &read_fds, nullptr, nullptr, &timeout);

    if (rv > 0 && FD_ISSET(fd_, &read_fds)) {
        int bytes_read = read(fd_, buffer, sizeof(buffer));
        if (bytes_read > 0) {
            result.assign(buffer, bytes_read);
        }
    }

    return result;
}

// MCU数据管理功能实现
void SerialPort::initRos(ros::NodeHandle& nh) {
    // 订阅下行数据话题
    downstreamSub_ = nh.subscribe("/serial/downstream", 10, &SerialPort::mcuCommandCallback, this);
    
    // 创建上行数据发布器
    createPublishers(nh);
    
    ROS_INFO("SerialPort ROS communication initialized");
    ROS_INFO("Listening on /serial/downstream for downstream data");
}

void SerialPort::processReceivedData(const std::string& data) {
    if (data.empty()) return;
    
    receiveBuffer_ += data;
    
    std::string packet;
    while (extractPacketFromBuffer(receiveBuffer_, packet)) {
        // 解析JSON数据
        parseJsonMessage(packet);
        
        ROS_INFO("MCU JSON数据: %s", packet.c_str());
    }
}

bool SerialPort::extractPacketFromBuffer(std::string& buffer, std::string& packet) {
    size_t start = buffer.find('{');
    if (start == std::string::npos) {
        buffer.clear(); // 清空无效数据
        return false;
    }
    
    // 寻找匹配的右大括号
    int braceCount = 0;
    size_t end = start;
    for (size_t i = start; i < buffer.length(); ++i) {
        if (buffer[i] == '{') {
            braceCount++;
        } else if (buffer[i] == '}') {
            braceCount--;
            if (braceCount == 0) {
                end = i;
                break;
            }
        }
    }
    
    if (braceCount != 0) {
        return false; // JSON数据包未完整
    }
    
    packet = buffer.substr(start, end - start + 1);
    buffer.erase(0, end + 1); // 移除已处理的数据
    
    return true;
}

void SerialPort::updateMcuData() {
    std::string data = receiveData(1);
    if (!data.empty()) {
        processReceivedData(data);
    }
}

void SerialPort::mcuCommandCallback(const std_msgs::String::ConstPtr& msg) {
    std::string command = msg->data;
    sendData(command);
    ROS_INFO("发送到MCU: %s", command.c_str());
}


void SerialPort::createPublishers(ros::NodeHandle& nh) {
    // 为每个预定义的键创建对应的发布器
    for (const auto& pair : keyDataTypes_) {
        const std::string& key = pair.first;
        const std::string& dataType = pair.second;
        
        if (dataType == "string") {
            std::string topicName = "/serial/upstream/" + key;
            stringPublishers_[key] = nh.advertise<std_msgs::String>(topicName, 10);
            ROS_INFO("创建字符串话题: %s", topicName.c_str());
        } else if (dataType == "double") {
            std::string topicName = "/serial/upstream/" + key;
            doublePublishers_[key] = nh.advertise<std_msgs::Float64>(topicName, 10);
            ROS_INFO("创建数值话题: %s", topicName.c_str());
        }
    }
}

void SerialPort::publishUpstreamData(const std::string& key, const std::string& value) {
    auto typeIt = keyDataTypes_.find(key);
    if (typeIt == keyDataTypes_.end()) {
        // 未预定义的键，默认作为字符串处理
        std::string topicName = "/serial/upstream/" + key;
        if (stringPublishers_.find(key) == stringPublishers_.end()) {
            ROS_WARN("未预定义的键 '%s'，将作为字符串类型处理", key.c_str());
            // 这里可以选择动态创建发布器，但为了简化，暂时跳过
            return;
        }
    }
    
    const std::string& dataType = typeIt->second;
    
    if (dataType == "string") {
        auto pubIt = stringPublishers_.find(key);
        if (pubIt != stringPublishers_.end()) {
            std_msgs::String msg;
            msg.data = value;
            pubIt->second.publish(msg);
            ROS_DEBUG("发布字符串数据: %s = %s", key.c_str(), value.c_str());
        }
    } else if (dataType == "double") {
        auto pubIt = doublePublishers_.find(key);
        if (pubIt != doublePublishers_.end()) {
            try {
                double doubleValue = std::stod(value);
                std_msgs::Float64 msg;
                msg.data = doubleValue;
                pubIt->second.publish(msg);
                ROS_DEBUG("发布数值数据: %s = %f", key.c_str(), doubleValue);
            } catch (const std::exception& e) {
                ROS_WARN("无法将值转换为数值: %s = %s", key.c_str(), value.c_str());
            }
        }
    }
}

// JSON数据解析功能
void SerialPort::parseJsonMessage(const std::string& jsonString) {
    try {
        json j = json::parse(jsonString);
        std::lock_guard<std::mutex> lock(dataMutex_);
        
        // 遍历JSON对象中的所有键值对
        for (auto it = j.begin(); it != j.end(); ++it) {
            std::string key = it.key();
            auto& value = it.value();
            
            std::string valueStr;
            if (value.is_string()) {
                valueStr = value.get<std::string>();
            } else if (value.is_number_integer()) {
                valueStr = std::to_string(value.get<int>());
            } else if (value.is_number_float()) {
                valueStr = std::to_string(value.get<double>());
            } else if (value.is_boolean()) {
                valueStr = value.get<bool>() ? "1" : "0";
            } else {
                valueStr = value.dump(); // 其他类型转为JSON字符串
            }
            
            // 存储键值对
            mcuData_[key] = valueStr;
            ROS_DEBUG("JSON数据更新: %s = %s", key.c_str(), valueStr.c_str());
            
            // 发布上行数据到对应话题
            publishUpstreamData(key, valueStr);
        }
        
    } catch (const std::exception& e) {
        ROS_WARN("解析JSON消息失败: %s, 消息内容: %s", e.what(), jsonString.c_str());
    }
}

// 数据查询接口实现
bool SerialPort::hasKey(const std::string& key) const {
    std::lock_guard<std::mutex> lock(dataMutex_);
    return mcuData_.find(key) != mcuData_.end();
}

std::string SerialPort::getValue(const std::string& key) const {
    std::lock_guard<std::mutex> lock(dataMutex_);
    auto it = mcuData_.find(key);
    return (it != mcuData_.end()) ? it->second : "";
}

int SerialPort::getIntValue(const std::string& key) const {
    std::string value = getValue(key);
    if (value.empty()) return 0;
    try {
        return std::stoi(value);
    } catch (const std::exception&) {
        ROS_WARN("无法转换为整数: %s = %s", key.c_str(), value.c_str());
        return 0;
    }
}

double SerialPort::getDoubleValue(const std::string& key) const {
    std::string value = getValue(key);
    if (value.empty()) return 0.0;
    try {
        return std::stod(value);
    } catch (const std::exception&) {
        ROS_WARN("无法转换为双精度: %s = %s", key.c_str(), value.c_str());
        return 0.0;
    }
}

std::map<std::string, std::string> SerialPort::getAllData() const {
    std::lock_guard<std::mutex> lock(dataMutex_);
    return mcuData_;
}

void SerialPort::loadUpstreamConfig(const std::string& configFile) {
    ROS_INFO("正在加载MCU上行数据配置: %s", configFile.c_str());
    
    try {
        YAML::Node config = YAML::LoadFile(configFile);
        
        keyDataTypes_.clear(); // 清空现有配置
        
        int loadedCount = 0;
        for (const auto& item : config) {
            std::string keyName = item.first.as<std::string>();
            
            if (item.second.IsMap()) {
                YAML::Node keyConfig = item.second;
                
                // 获取数据类型
                std::string dataType = "double"; // 默认类型
                if (keyConfig["type"]) {
                    dataType = keyConfig["type"].as<std::string>();
                }
                
                // 验证数据类型
                if (dataType != "string" && dataType != "double") {
                    ROS_WARN("配置键 '%s' 的数据类型 '%s' 无效，使用默认类型 'double'", 
                             keyName.c_str(), dataType.c_str());
                    dataType = "double";
                }
                
                // 存储配置
                keyDataTypes_[keyName] = dataType;
                loadedCount++;
                
                // 打印调试信息
                std::string description = keyConfig["description"] ? 
                    keyConfig["description"].as<std::string>() : "无描述";
                std::string topic = keyConfig["topic"] ? 
                    keyConfig["topic"].as<std::string>() : keyName;
                
                ROS_DEBUG("加载数据键: %s (类型: %s, 话题: %s, 描述: %s)", 
                         keyName.c_str(), dataType.c_str(), topic.c_str(), description.c_str());
            } else {
                ROS_WARN("跳过无效配置项: %s", keyName.c_str());
            }
        }
        
        ROS_INFO("成功从配置文件加载了 %d 个数据键配置", loadedCount);
        
        if (loadedCount == 0) {
            throw std::runtime_error("配置文件中没有找到有效的数据键配置");
        }
        
    } catch (const YAML::Exception& e) {
        throw std::runtime_error("YAML解析错误: " + std::string(e.what()));
    } catch (const std::exception& e) {
        throw std::runtime_error("配置文件加载错误: " + std::string(e.what()));
    }
}
